#include "currsense.h"
#include "driver/gpio.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

// ADC 相关常量
#define ADC_VOLTAGE      3.3f               // ADC参考电压（V）
#define ADC_RESOLUTION   4095.0f            // ADC 12位精度
#define ADC_CONV         (ADC_VOLTAGE / ADC_RESOLUTION)  // 原始值转电压

/**
 * @brief 初始化 CurrSense（设置电机通道、增益参数并执行校准）
 */
void currsense_init(CurrSense* cs, int mot_num) {
    // 基本电路参数
    cs->shunt_resistor = 0.01f;               // 分流电阻 10mΩ
    cs->amp_gain = 50.0f;                     // 放大器增益 50x
    cs->volts_to_amps_ratio = 1.0f / (cs->shunt_resistor * cs->amp_gain);

    cs->gain_a = cs->volts_to_amps_ratio;
    cs->gain_b = cs->volts_to_amps_ratio;
    cs->gain_c = cs->volts_to_amps_ratio;

    // 根据电机编号选择 ADC 通道
    if (mot_num == 0) {
        cs->pinA = ADC1_CHANNEL_3; // GPIO39
        cs->pinB = ADC1_CHANNEL_0; // GPIO36
        cs->pinC = NOT_SET_CH;     // 未连接 C 相
    } else if (mot_num == 1) {
        cs->pinA = ADC1_CHANNEL_7; // GPIO35
        cs->pinB = ADC1_CHANNEL_6; // GPIO34
        cs->pinC = NOT_SET_CH;
    }

    // 初始化 ADC 硬件和校准
    currsense_configure_adc(cs);
    currsense_calibrate(cs);
}

/**
 * @brief 配置 ADC 通道精度与衰减
 */
void currsense_configure_adc(CurrSense* cs) {
    adc1_config_width(ADC_WIDTH_BIT_12); // 12位精度
    adc1_config_channel_atten(cs->pinA, ADC_ATTEN_DB_11); // 支持最大3.3V
    adc1_config_channel_atten(cs->pinB, ADC_ATTEN_DB_11);
    if (IS_VALID_CH(cs->pinC)) {
        adc1_config_channel_atten(cs->pinC, ADC_ATTEN_DB_11);
    }
}

/**
 * @brief 读取指定 ADC 通道的电压值
 */
float currsense_read_voltage(CurrSense* cs, adc1_channel_t channel) {
    int raw = adc1_get_raw(channel);
    return raw * ADC_CONV;
}

/**
 * @brief 对 ADC 通道执行偏移校准（空载状态下）
 */
void currsense_calibrate(CurrSense* cs) {
    const int rounds = 1000;

    cs->offset_ia = 0;
    cs->offset_ib = 0;
    cs->offset_ic = 0;

    for (int i = 0; i < rounds; i++) {
        // 连续采样求平均电压偏移
        cs->offset_ia += currsense_read_voltage(cs, cs->pinA);
        cs->offset_ib += currsense_read_voltage(cs, cs->pinB);
        if (IS_VALID_CH(cs->pinC)) {
            cs->offset_ic += currsense_read_voltage(cs, cs->pinC);
        }
        vTaskDelay(pdMS_TO_TICKS(1));
    }

    cs->offset_ia /= rounds;
    cs->offset_ib /= rounds;

    if (IS_VALID_CH(cs->pinC)) {
        cs->offset_ic /= rounds;
    } else {
        // 若未启用 C 相传感器，避免未初始化使用
        cs->offset_ic = (cs->offset_ia + cs->offset_ib);
        // cs->offset_ic = 2.6;
        // ESP_LOGI("","=========================== ");        
    }
}

/**
 * @brief 获取当前三相电流值（单位 A）
 *        如果 C 相未连接，则根据 Ia + Ib + Ic = 0 推导 Ic
 */
void currsense_get_currents(CurrSense* cs) {
    cs->current_a = (currsense_read_voltage(cs, cs->pinA) - cs->offset_ia) * cs->gain_a;
    cs->current_b = (currsense_read_voltage(cs, cs->pinB) - cs->offset_ib) * cs->gain_b;

    if (IS_VALID_CH(cs->pinC)) {
        cs->current_c = (currsense_read_voltage(cs, cs->pinC) - cs->offset_ic) * cs->gain_c;
    } else {
        // 使用基尔霍夫定律（电流守恒）推导 C 相
        cs->current_c = - (cs->current_a + cs->current_b);
    }
}
